﻿/*
 * This file is part of MonoStrategy.
 *
 * Copyright (C) 2010-2011 Christoph Husse
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Affero General Public License as
 *  published by the Free Software Foundation, either version 3 of the
 *  License, or (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Affero General Public License for more details.
 *
 *  You should have received a copy of the GNU Affero General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Authors: 
 *      # Christoph Husse
 * 
 * Also checkout our homepage: http://monostrategy.codeplex.com/
 */
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MonoStrategy
{
    internal partial class TerrainRouting
    {
        private class RoutingNodeAStar
        {
            private OpenCloseMap m_ClosedSet;
            private OpenCloseMap m_OpenSet;
            private PriorityQueue<RoutingNode> m_OrderedOpenSet;
            private OpenCloseMap m_RuntimeGrid;
            private int m_Size;
            RoutingNode m_StartNode;

            public TerrainRouting SearchSpace { get; private set; }

            public RoutingNodeAStar(TerrainRouting inParent)
            {
                SearchSpace = inParent;
                m_Size = inParent.Size;
                m_Size = inParent.Size;
                m_ClosedSet = new OpenCloseMap(m_Size, m_Size);
                m_OpenSet = new OpenCloseMap(m_Size, m_Size);
                m_RuntimeGrid = new OpenCloseMap(m_Size, m_Size);
                m_OrderedOpenSet = new PriorityQueue<RoutingNode>();
            }

            private Double Heuristic(RoutingNode inStart, RoutingNode inEnd)
            {
                return inStart.CellXY.DistanceTo(inEnd.CellXY);
            }

            /// <summary>
            /// Returns null, if no path is found. Start- and End-Node are included in returned path. The user context
            /// is passed to IsWalkable().
            /// </summary>
            public void Initialize(RoutingNode inStartNode)
            {
                if (inStartNode == null)
                    throw new ArgumentNullException();

                m_StartNode = inStartNode;

                m_ClosedSet.Clear();
                m_OpenSet.Clear();
                m_RuntimeGrid.Clear();
                m_OrderedOpenSet.Clear();

                m_StartNode.G = 0;
                m_StartNode.H = 0;
                m_StartNode.F = m_StartNode.H;

                m_OpenSet.Add(m_StartNode);
                m_OrderedOpenSet.Push(m_StartNode);

                m_RuntimeGrid.Add(m_StartNode);
            }

            public bool GetHeuristic(RoutingNode inForNode, out Double outHeuristic)
            {
                if (m_StartNode.CellXY == inForNode.CellXY)
                {
                    outHeuristic = 0;
                    return true;
                }

                if (m_ClosedSet.Contains(inForNode))
                {
                    outHeuristic = m_ClosedSet[inForNode].G;
                    return true;
                }

                while (!m_OpenSet.IsEmpty)
                {
                    RoutingNode x = m_OrderedOpenSet.Pop();

                    m_OpenSet.Remove(x);
                    m_ClosedSet.Add(x);

                    for (int i = 0; i < x.Neighbors.Length; i++)
                    {
                        RoutingNodeLink yLink = x.Neighbors[i];
                        Boolean tentative_is_better;

                        if (yLink == null)
                            continue;

                        RoutingNode y = yLink.Node;

                        if (m_ClosedSet.Contains(y))
                            continue;

                        Double tentative_g_score = m_RuntimeGrid[x].G + yLink.Costs;
                        Boolean wasAdded = false;

                        if (!m_OpenSet.Contains(y))
                        {
                            m_OpenSet.Add(y);
                            tentative_is_better = true;
                            wasAdded = true;
                        }
                        else if (tentative_g_score < m_RuntimeGrid[y].G)
                        {
                            tentative_is_better = true;
                        }
                        else
                        {
                            tentative_is_better = false;
                        }

                        if (tentative_is_better)
                        {
                            if (!m_RuntimeGrid.Contains(y))
                                m_RuntimeGrid.Add(y);

                            m_RuntimeGrid[y].G = tentative_g_score;
                            m_RuntimeGrid[y].H = Heuristic(y, inForNode);
                            m_RuntimeGrid[y].F = m_RuntimeGrid[y].G + m_RuntimeGrid[y].H;

                            if (wasAdded)
                                m_OrderedOpenSet.Push(y);
                            else
                                m_OrderedOpenSet.Update(y);
                        }
                    }

                    if (x.CellXY == inForNode.CellXY)
                    {
                        outHeuristic = inForNode.G;
                        return true;
                    }
                }

                outHeuristic = Double.NaN;
                return false;
            }

            private class OpenCloseMap
            {
                private RoutingNode[,] m_Map;
                public int Width { get; private set; }
                public int Height { get; private set; }
                public int Count { get; private set; }

                public RoutingNode this[Int32 x, Int32 y]
                {
                    get
                    {
                        return m_Map[x, y];
                    }
                }

                public RoutingNode this[RoutingNode Node]
                {
                    get
                    {
                        return m_Map[Node.CellXY.X, Node.CellXY.Y];
                    }

                }

                public bool IsEmpty
                {
                    get
                    {
                        return Count == 0;
                    }
                }

                public OpenCloseMap(int inWidth, int inHeight)
                {
                    m_Map = new RoutingNode[inWidth, inHeight];
                    Width = inWidth;
                    Height = inHeight;
                }

                public void Add(RoutingNode inValue)
                {
                    RoutingNode item = m_Map[inValue.CellXY.X, inValue.CellXY.Y];

#if DEBUG
                    if (item != null)
                        throw new ApplicationException();
#endif

                    Count++;
                    m_Map[inValue.CellXY.X, inValue.CellXY.Y] = inValue;
                }

                public bool Contains(RoutingNode inValue)
                {
                    RoutingNode item = m_Map[inValue.CellXY.X, inValue.CellXY.Y];

                    if (item == null)
                        return false;

#if DEBUG
                    if (!inValue.Equals(item))
                        throw new ApplicationException();
#endif

                    return true;
                }

                public void Remove(RoutingNode inValue)
                {
                    RoutingNode item = m_Map[inValue.CellXY.X, inValue.CellXY.Y];

#if DEBUG
                    if (!inValue.Equals(item))
                        throw new ApplicationException();
#endif

                    Count--;
                    m_Map[inValue.CellXY.X, inValue.CellXY.Y] = null;
                }

                public void Clear()
                {
                    Count = 0;

                    for (int x = 0; x < Width; x++)
                    {
                        for (int y = 0; y < Height; y++)
                        {
                            m_Map[x, y] = null;
                        }
                    }
                }
            }
        }
    }
}